#include "ddsmodular.h"

MQP Publisher::m_mqp;
int Publisher::m_num = 0;
DDSPublisher* Publisher::m_publisher = nullptr;
DDSDomainParticipant* Publisher::m_participant = nullptr;

MQS Subscriber::m_mqs;
int Subscriber::m_num = 0;
DDSSubscriber* Subscriber::m_subscriber = nullptr;
DDSDomainParticipant* Subscriber::m_participant = nullptr;

int Publisher::shutdown() {
    DDS_ReturnCode_t m_retcode;
    int status = 0;

    if (m_participant != NULL) {
        m_retcode = m_participant->delete_contained_entities();
        if (m_retcode != DDS_RETCODE_OK) {
            fprintf(stderr, "delete_contained_entities error %d\n", m_retcode);
            status = -1;
        }

        m_retcode = DDSTheParticipantFactory->delete_participant(m_participant);
        if (m_retcode != DDS_RETCODE_OK) {
            fprintf(stderr, "delete_m_participant error %d\n", m_retcode);
            status = -1;
        }
    }

    return status;
}

int Publisher::initPublisher(int domainId = 0) {
    m_participant = DDSTheParticipantFactory->create_participant(
            domainId, DDS_PARTICIPANT_QOS_DEFAULT,
            NULL, DDS_STATUS_MASK_NONE);
    if (m_participant == NULL) {
        cout << "create_m_participant error";
        Publisher::shutdown();
        return -1;
    }

    m_publisher = m_participant->create_publisher(
        DDS_PUBLISHER_QOS_DEFAULT, NULL, DDS_STATUS_MASK_NONE);
    if (m_publisher == NULL) {
        cout << "create_publisher error";
        Publisher::shutdown();
        return -1;
    }

    return 0;
}

Publisher::Publisher() {
    CSMXPv3_writer = nullptr;
    m_num++;
    if(m_publisher == nullptr || m_num == 0) {
        if(Publisher::initPublisher(0)) {
            cout << "init error!";
            return;
        }
    }
}

Publisher::~Publisher() {
    m_num--;
    if(!m_num) {
        for(auto it = m_mqp.begin(); it != m_mqp.end(); ++it) {
            deleteTopic(it.key());
        }
        m_mqp.clear();
        Publisher::shutdown();
    }
}

void Publisher::createTopic(const std::string& top, bool reliable, bool redundancy,  const int& priority) {
    if(m_mqp.find(top) != m_mqp.end()) {
        cout << "已存在该话题，可以直接发布相关信息";
        return;
    }

    DDSMPub ddsm;
    const char *type_name = nullptr;

    type_name = CSMXPv3TypeSupport::get_type_name();
    m_retcode = CSMXPv3TypeSupport::register_type(
        m_participant, type_name);
    if (m_retcode != DDS_RETCODE_OK) {
        cout << "register_type error " << m_retcode;
        Publisher::shutdown();
        return;
    }

    ddsm.topic = m_participant->create_topic(
        top.c_str(),
        type_name, DDS_TOPIC_QOS_DEFAULT, NULL, DDS_STATUS_MASK_NONE);
    if (ddsm.topic == nullptr) {
        cout << "create_topic error";
        Publisher::shutdown();
        return;
    }

    DDS_DataWriterQos reliableqos;

    if(m_participant->get_default_datawriter_qos(reliableqos)!= DDS_RETCODE_OK) {
        cout << "error get_default_datawriter_qos ";
    }
    if(reliable) {
        reliableqos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
        reliableqos.liveliness.lease_duration = DDS_Duration_t::from_millis(30);
        reliableqos.liveliness.assertions_per_lease_duration = 3;
    }
    else {
        reliableqos.reliability.kind = DDS_BEST_EFFORT_RELIABILITY_QOS;
    }
    if(redundancy) {
        if(priority != 1 && priority != 2) {
            cout << "未指定正确的优先级！";
            return;
        }
        reliableqos.transport_priority.value = priority;
        reliableqos.ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
        reliableqos.liveliness.kind = DDS_MANUAL_BY_TOPIC_LIVELINESS_QOS;
        reliableqos.liveliness.lease_duration = DDS_Duration_t::from_millis(100);
    }

    ddsm.writer = m_publisher->create_datawriter(
        ddsm.topic, reliableqos, NULL, DDS_STATUS_MASK_NONE);
    if (ddsm.writer == NULL) {
        cout << "create_datawriter error";
        Publisher::shutdown();
        return;
    }

    CSMXPv3_writer = CSMXPv3DataWriter::narrow(ddsm.writer);
    if (CSMXPv3_writer == NULL) {
        cout << "DataWriter narrow error";
        Publisher::shutdown();
        return;
    }


    ddsm.instance = CSMXPv3TypeSupport::create_data();
    if (ddsm.instance == NULL) {
        cout << "CSMXPv3TypeSupport::create_data error";
        Publisher::shutdown();
        return;
    }

    m_mqp.insert(top, ddsm);
    cout << "创建话题成功~";
}

void Publisher::publish4Topic(const std::string& top, const TupMsg& tmsg) {
    if(m_mqp.find(top) == m_mqp.end()) {
        cout << "不存在该话题，请添加...";
        return;
    }

    m_mqp[top].instance->src = std::get<0>(tmsg);
    m_mqp[top].instance->dst = std::get<1>(tmsg);
    std::vector<char> ilist = std::get<2>(tmsg);
    int size = ilist.size();
    DDS_OctetSeq_ensure_length(&(m_mqp[top].instance->msg), size, 1450);
    DDS_Octet* ddsOctet = m_mqp[top].instance->msg.get_contiguous_buffer();
    int index = 0;
    for(auto it = ilist.begin(); it != ilist.end(); ++it) {
        ddsOctet[index] = *it;
        index++;
    }

    DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL;
    m_retcode = CSMXPv3_writer->write(*(m_mqp[top].instance), instance_handle);
    if (m_retcode != DDS_RETCODE_OK) {
        cout << "write error " << m_retcode;
    }
}

void Publisher::deleteTopic(const std::string& topic) {
    if(m_mqp.contains(topic)) {
        DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL;
        m_retcode = CSMXPv3_writer->unregister_instance(
            *m_mqp[topic].instance, instance_handle);
        if (m_retcode != DDS_RETCODE_OK) {
            cout << "unregister instance error " << m_retcode;
            return;
        }

        m_retcode = CSMXPv3TypeSupport::delete_data(m_mqp[topic].instance);
        if (m_retcode != DDS_RETCODE_OK) {
            cout << "CSMXPv3TypeSupport::delete_data error " << m_retcode;
            return;
        }

        m_retcode = m_publisher->delete_datawriter(m_mqp[topic].writer);
        if (m_retcode != DDS_RETCODE_OK) {
            cout << "CSMXPv3TypeSupport::delete_writer error " << m_retcode;
            return;
        }

        m_retcode = m_participant->delete_topic(m_mqp[topic].topic);
        if (m_retcode != DDS_RETCODE_OK) {
            cout << "CSMXPv3TypeSupport::delete_topic error " << m_retcode;
            return;
        }

        if(m_num) {
            m_mqp.erase(m_mqp.find(topic));
        }

        cout << "删除话题成功！";
    }
    else {
        cout << "不存在该主题，删除失败！";
    }
}


void CSMXPv3Listener::on_data_available(DDSDataReader* reader)
{
    TupMsg remsg;
    CSMXPv3DataReader *CSMXPv3_reader = NULL;
    CSMXPv3Seq data_seq;
    DDS_SampleInfoSeq info_seq;
    DDS_ReturnCode_t retcode;
    int i;

    CSMXPv3_reader = CSMXPv3DataReader::narrow(reader);
    if (CSMXPv3_reader == NULL) {
        cout << "DataReader narrow error";
        return;
    }

    retcode = CSMXPv3_reader->take(
        data_seq, info_seq, DDS_LENGTH_UNLIMITED,
        DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE);

    if (retcode == DDS_RETCODE_NO_DATA) {
        return;
    } else if (retcode != DDS_RETCODE_OK) {
        cout << "take error " << retcode;
        return;
    }

    for (i = 0; i < data_seq.length(); ++i) {
        if (info_seq[i].valid_data) {
            DDS_OctetSeq seq = data_seq[i].msg;
            int l = seq.length();

            std::vector<char> tmp;
            for(int j = 0; j < l; ++j) {
                tmp.push_back(data_seq[i].msg.get_contiguous_buffer()[j]);
            }
            ushort tusrc = data_seq[i].src;
            ushort tudst = data_seq[i].dst;
            remsg = std::make_tuple(tusrc, tudst, tmp);
            m_fun(remsg);
            CSMXPv3TypeSupport::print_data(&data_seq[i]);
        }
    }

    retcode = CSMXPv3_reader->return_loan(data_seq, info_seq);
    if (retcode != DDS_RETCODE_OK) {
        cout << "return loan error " << retcode;
    }
}


int Subscriber::shutdown() {
    DDS_ReturnCode_t retcode;
    int status = 0;

    if (m_participant != NULL) {
        retcode = m_participant->delete_contained_entities();
        if (retcode != DDS_RETCODE_OK) {
            cout << "delete_contained_entities error %d\n" << retcode;
            status = -1;
        }

        retcode = DDSTheParticipantFactory->delete_participant(m_participant);
        if (retcode != DDS_RETCODE_OK) {
            cout << "delete_participant error %d\n" << retcode;
            status = -1;
        }
    }
    return status;
}

void Subscriber::initSubcriber() {
    m_participant = DDSTheParticipantFactory->create_participant(
        0, DDS_PARTICIPANT_QOS_DEFAULT,
        NULL, DDS_STATUS_MASK_NONE);
    if (m_participant == NULL) {
        cout << "create_participant error";
        Subscriber::shutdown();
        return;
    }

    m_subscriber = m_participant->create_subscriber(
        DDS_SUBSCRIBER_QOS_DEFAULT, NULL, DDS_STATUS_MASK_NONE);
    if (m_subscriber == NULL) {
        cout << "create_subscriber error";
        Subscriber::shutdown();
        return;
    }
}

Subscriber::Subscriber() {
    m_num++;
    if(m_num == 0 || m_participant == nullptr) {
        Subscriber::initSubcriber();
    }
}

Subscriber::~Subscriber() {
    m_num--;
    if(!m_num) {
        for(auto it = m_mqs.begin(); it != m_mqs.end(); ++it) {
            cancelTopic(it.key());
        }
        m_mqs.clear();
        Subscriber::shutdown();
    }
}

void Subscriber::listenTopic(const std::string& topic, Func fun, bool reliable, bool redundancy) {
    if(m_mqs.contains(topic)) {
        cout << "已订阅，不需要重复订阅！";
        return;
    }

    DDSMSub ddsm;
    const char *type_name = nullptr;
    CSMXPv3Listener *reader_listener = nullptr;

    type_name = CSMXPv3TypeSupport::get_type_name();
    m_retcode = CSMXPv3TypeSupport::register_type(
        m_participant, type_name);
    if (m_retcode != DDS_RETCODE_OK) {
        cout << "register_type error " << m_retcode;
        Subscriber::shutdown();
        return;
    }

    ddsm.topic = m_participant->create_topic(
        topic.c_str(),
        type_name, DDS_TOPIC_QOS_DEFAULT, NULL,
        DDS_STATUS_MASK_NONE);
    if (ddsm.topic == NULL) {
        cout << "create_topic error";
        Subscriber::shutdown();
        return;
    }

    DDS_DataReaderQos reliableqos;
    if(m_participant->get_default_datareader_qos(reliableqos)!= DDS_RETCODE_OK) {
        cout << "error get ";
    }


    if(reliable) {
        reliableqos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
    }
    else{
        reliableqos.reliability.kind = DDS_BEST_EFFORT_RELIABILITY_QOS;
    }
    if(redundancy) {
        reliableqos.ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
        reliableqos.liveliness.kind = DDS_MANUAL_BY_TOPIC_LIVELINESS_QOS;
        reliableqos.liveliness.lease_duration = DDS_Duration_t::from_millis(300);
    }

    reader_listener = new CSMXPv3Listener(fun);

    ddsm.reader = m_subscriber->create_datareader(
                ddsm.topic, reliableqos, reader_listener,
                DDS_STATUS_MASK_ALL);

    if (ddsm.reader == NULL) {
        cout << "create_datareader error";
        Subscriber::shutdown();
        delete reader_listener;
        return;
    }

    m_mqs.insert(topic, ddsm);

    cout << "订阅成功~";
}

void Subscriber::cancelTopic(const std::string& topic) {
    if(!m_mqs.contains(topic)) {
        cout << "不存在该话题，不能取消~";
        return;
    }

    m_retcode = m_subscriber->delete_datareader(m_mqs[topic].reader);
    if(m_retcode != DDS_RETCODE_OK) {
        cout << "delete_datareader error " << m_retcode;
        return;
    }

    m_retcode = m_participant->delete_topic(m_mqs[topic].topic);
    if(m_retcode != DDS_RETCODE_OK) {
        cout << "delete_topic error " << m_retcode;
        return;
    }

    if(m_num) {
        m_mqs.erase(m_mqs.find(topic));
    }

    cout << "取消订阅成功！";
}
